#include <ceres/ceres.h>
#include <ceres/rotation.h>
#include <Eigen/Dense>
#include <iostream>
#include <vector>
#include <random>

// 3D-2D 重投影误差类
class ReprojectionError {
public:
    ReprojectionError(double observed_x, double observed_y, 
                     double fx, double fy, double cx, double cy)
        : observed_x_(observed_x), observed_y_(observed_y),
          fx_(fx), fy_(fy), cx_(cx), cy_(cy) {}

    template<typename T>
    bool operator()(const T* const camera_rotation,
                    const T* const camera_translation,
                    const T* const point_3d,
                    T* residuals) const {
        // 相机旋转 (四元数形式: [w, x, y, z])
        T p[3];
        
        // 使用四元数旋转点
        T point_relative[3];
        point_relative[0] = point_3d[0] - camera_translation[0];
        point_relative[1] = point_3d[1] - camera_translation[1];
        point_relative[2] = point_3d[2] - camera_translation[2];
        
        // 四元数旋转: q * p * q^{-1}
        ceres::QuaternionRotatePoint(camera_rotation, point_relative, p);
        
        // 投影到图像平面
        T xp = fx_ * p[0] / p[2] + cx_;
        T yp = fy_ * p[1] / p[2] + cy_;
        
        // 计算残差
        residuals[0] = observed_x_ - xp;
        residuals[1] = observed_y_ - yp;
        
        // std::cout << "residuals: " << residuals[0] << residuals[1] << std::endl;

        return true;
    }
    
    static ceres::CostFunction* Create(double observed_x, double observed_y,
                                      double fx, double fy, double cx, double cy) {
        return new ceres::AutoDiffCostFunction<ReprojectionError, 2, 4, 3, 3>(
            new ReprojectionError(observed_x, observed_y, fx, fy, cx, cy));
    }

private:
    double observed_x_, observed_y_;  // 观测到的2D点
    double fx_, fy_, cx_, cy_;        // 相机内参
};
